#ifndef RM_DYNAMICS_ARM_H
#define RM_DYNAMICS_ARM_H

#include "rm_dynamics_arm_global.h"
#include "Kinematics_API/define.h"
#include <stdint.h>
#include <math.h>
#define  M_PI_RAD    0.0174533f
#define  MI_PI_ANG   57.2957805f
#define  PI          3.14159f
#define  DELTA       0.26f   //关节判断角度差
#define  DELTA2      2*PI    //关节运动到该处

//DH 参数
#define dbs   ((float)0.2405f)
#define dse   ((float)0.256f)
#define dew   ((float)0.210f)

//#define dbs   ((float)0.2405f)
//#define dse   ((float)0.256f)
//#define dse   ((float)0.256263f)
//#define dew   ((float)0.210f)
//#define dew   ((float)0.209729f)

//RM-75F
//#define dwt   ((float)0.1725f)
//RM-75
#define dwt   ((float)0.144f)
#define dwt_ZF  ((float)0.1642f)
#define dwt_SF  ((float)0.1725f)

float lwt  = dwt;

#define d4    ((float)0.0f)
/*#define angleV ((float) -0.3333333333f * M_PI)*///绕Y轴旋转
//#define angleV ((float) 0.0f * M_PI)
float angleV = ((float) 0.0f);
//#define angleU ((float) 0.5f * M_PI)   //右臂侧装
//#define angleU ((float) -0.5f * M_PI)  //左臂侧装
float angleU = ((float) 0.0f);          //竖直安装

#define m1 ((float)1.688f)//kg
#define m2 ((float)1.029f)
#define m3 ((float)0.999f)
#define m4 ((float)0.673f)
#define m5 ((float)0.653f)
#define m6 ((float)0.646f)
#define m7 ((float)0.109f)

//关节限位
#define theta1_u    PI    //175
#define theta1_l   -PI
#define theta2_u   2.2689f       //115
#define theta2_l   -2.2689f      //-115
#define theta3_u   PI
#define theta3_l   -PI
#define theta4_u    2.3562f
#define theta4_l   -2.3562f
#define theta5_u   PI
#define theta5_l   -PI
#define theta6_u    2.234f
#define theta6_l   -2.234f
#define theta7_u   2 * M_PI
#define theta7_l   -2 * M_PI

//关节限位
#define JOINT1_LOWER   -180
#define JOINT1_UPPER   180
#define JOINT2_LOWER  -130
#define JOINT2_UPPER  130
#define JOINT3_LOWER   -180
#define JOINT3_UPPER   180
#define JOINT4_LOWER   -135
#define JOINT4_UPPER   135
#define JOINT5_LOWER   -180
#define JOINT5_UPPER   180
#define JOINT6_LOWER   -128
#define JOINT6_UPPER   128
#define JOINT7_LOWER   -360
#define JOINT7_UPPER   360

#define M_PI		3.14159265358979323846

RM_DYNAMICS_ARMSHARED_EXPORT void RM_Init();

RM_DYNAMICS_ARMSHARED_EXPORT void setAngle(float x,float y);

RM_DYNAMICS_ARMSHARED_EXPORT float my_atan2f(const float y, const float x);

RM_DYNAMICS_ARMSHARED_EXPORT void Homogeneous_transformation_matrix(float  * joint,Matrix *T);

RM_DYNAMICS_ARMSHARED_EXPORT Matrix Forward_Kinematics_e(float  * joint);

RM_DYNAMICS_ARMSHARED_EXPORT Matrix matrix_m(Matrix T1, Matrix T2);

RM_DYNAMICS_ARMSHARED_EXPORT Matrix  matrix_inv(Matrix T);

RM_DYNAMICS_ARMSHARED_EXPORT KINEMATIC Forward_Kinematics(float * joint);

RM_DYNAMICS_ARMSHARED_EXPORT int inverse_kinematics(const float *q_in, const KINEMATIC *q_pose, float *q_out,const uint8_t flag);
#endif // RM_DYNAMICS_ARM_H
